%%
clc
clear

nAngle =1;          % stroke 1; deviation 2; pitch 3
nVar = 4;            % angle 1 ; velocity 2; torque 3, power 4
nType = 1;           % net 1 ; iner 2; aero 3
fileFormat = 'fig';
fileFormat2 = 'png';
area_l_bird1 = 11.36e-4;
area_r_bird1 = 11.46e-4;
area_l_bird2 = 11.47e-4;
area_r_bird2 = 12e-4;
v_tip_l_bird1 = 16.5;  % hover = 10.93
v_tip_r_bird1 = 15.96; % hover = 11.009
v_tip_l_bird2 = 18;  % hover = 11.7
v_tip_r_bird2 = 17.7; % hover = 11.7
density = 1.204; %kg/m^3 at 20 degC
powerCoef_l_bird1 = 0.5*density*(v_tip_l_bird1^3)*area_l_bird1;
powerCoef_r_bird1 = 0.5*density*(v_tip_r_bird1^3)*area_r_bird1;
powerCoef_l_bird2 = 0.5*density*(v_tip_l_bird2^3)*area_l_bird2;
powerCoef_r_bird2 = 0.5*density*(v_tip_r_bird2^3)*area_r_bird2;

 W_bird1 = 7.7e-3;
 W_bird2 = 7.5e-3; 
 g = 9.8;
 mgR_bird1 =  7.7*1e-3*g*7.5*1e-2;
 mgR_bird2 =  7.5*1e-3*g*7.5*1e-2;

% angles
if (nAngle == 1)
   ang = 'stroke';
elseif (nAngle == 2)
   ang = 'dev';
elseif (nAngle == 3)
   ang = 'pitch';
else
    disp('select a value between 1 to 3')
end

% variables
if (nVar == 1)
   var = 'ang';
   var1 = 'ang';

   bird1_l=load('angles_l_bird1.dat')*180/pi;
   bird1_r=load('angles_r_bird1.dat')*180/pi;
   bird2_l=load('angles_l_bird2.dat')*180/pi;
   bird2_r=load('angles_r_bird2.dat')*180/pi;
   bird2_l(:,1) = -bird2_l(:,1);
   bird2_r(:,1) = -bird2_r(:,1);
   bird2_l(:,3) = -bird2_l(:,3);
   bird2_r(:,3) = -bird2_r(:,3);
elseif (nVar == 2)
   var = 'vel';
   var1 = 'vel';

   bird1_l=load('angVel_l_bird1.dat');
   bird1_l(:,1) = -bird1_l(:,1);
   bird1_l(:,2) = -bird1_l(:,2);
   bird1_r=load('angVel_r_bird1.dat');
   bird2_l=load('angVel_l_bird2.dat');
   bird2_l(:,1) = -bird2_l(:,1);
   bird2_l(:,2) = -bird2_l(:,2);
   bird2_r=load('angVel_r_bird2.dat');
elseif (nVar == 3)
   if (nType == 1) 
      var = 'torque_net';
      var1 = 'torque net';

      bird1_l=load('totalInputT_l_bird1.dat')/mgR_bird1;
      bird1_l(:,1) = -bird1_l(:,1);
      bird1_l(:,2) = -bird1_l(:,2);
      bird1_r=load('totalInputT_r_bird1.dat')/mgR_bird1;
      bird2_l=load('totalInputT_l_bird2.dat')/mgR_bird2;
      bird2_l(:,1) = -bird2_l(:,1);
      bird2_l(:,2) = -bird2_l(:,2);
      bird2_r=load('totalInputT_r_bird2.dat')/mgR_bird2;
   elseif (nType == 2) 
      var = 'torque_iner';
      var1 = 'torque iner';

      bird1_l=load('totalInputTInertial_l_bird1.dat')/mgR_bird1;
      bird1_l(:,1) = -bird1_l(:,1);
      bird1_l(:,2) = -bird1_l(:,2);
      bird1_r=load('totalInputTInertial_r_bird1.dat')/mgR_bird1;
      bird2_l=load('totalInputTInertial_l_bird2.dat')/mgR_bird2;
      bird2_l(:,1) = -bird2_l(:,1);
      bird2_l(:,2) = -bird2_l(:,2);
      bird2_r=load('totalInputTInertial_r_bird2.dat')/mgR_bird2;
   elseif (nType == 3) 
      var = 'torque_aero';
      var1 = 'torque aero';
%     extra - sign for flipping aero torque
      bird1_l=(load('totalInputTAero_l_bird1.dat')/mgR_bird1);
      bird1_l(:,1) =-( -bird1_l(:,1));
      bird1_l(:,2) =-(-bird1_l(:,2));
      bird1_l(:,3) =-(bird1_l(:,3));
      bird1_r=-load('totalInputTAero_r_bird1.dat')/mgR_bird1;
      bird2_l=(load('totalInputTAero_l_bird2.dat')/mgR_bird2);
      bird2_l(:,1) = -(-bird2_l(:,1));
      bird2_l(:,2) = -(-bird2_l(:,2));
      bird2_l(:,3) = -(bird2_l(:,3));
      bird2_r=-load('totalInputTAero_r_bird2.dat')/mgR_bird2;
   else
      disp('select a torque type between 1 to 3')
   end
elseif (nVar == 4)
   if (nType == 1) 
      var = 'power_net';
      var1 = 'power net';

      bird1_l=load('InputPower_l_bird1.dat')/powerCoef_l_bird1;%W_bird1;
      bird1_r=load('InputPower_r_bird1.dat')/powerCoef_r_bird1;%W_bird1;
      bird2_l=load('InputPower_l_bird2.dat')/powerCoef_l_bird2;%W_bird2;
      bird2_r=load('InputPower_r_bird2.dat')/powerCoef_r_bird2 ;%W_bird2;
   elseif (nType == 2) 
      var = 'power_iner';
      var1 = 'power iner';

      bird1_l=load('InputPowerIner_l_bird1.dat')/powerCoef_l_bird1;%W_bird1;
      bird1_r=load('InputPowerIner_r_bird1.dat')/powerCoef_r_bird1;%W_bird1;
      bird2_l=load('InputPowerIner_l_bird2.dat')/powerCoef_l_bird2;%W_bird2;
      bird2_r=load('InputPowerIner_r_bird2.dat')/powerCoef_r_bird2;%W_bird2;
   elseif (nType == 3) 
      var = 'power_aero';
      var1 = 'power aero';

      bird1_l=-load('InputPowerAero_l_bird1.dat')/powerCoef_l_bird1;%W_bird1;
      bird1_r=-load('InputPowerAero_r_bird1.dat')/powerCoef_r_bird1;%W_bird1;
      bird2_l=-load('InputPowerAero_l_bird2.dat')/powerCoef_l_bird2;%/W_bird2;
      bird2_r=-load('InputPowerAero_r_bird2.dat')/powerCoef_r_bird2;%W_bird2;
   else
      disp('select a power type between 1 to 3')
   end
else
    disp('select a variable between 1 to 4')
end

fname1 = ['cycleAveraged/',ang,'_',var,'.',fileFormat];
fname2 = ['indiAveraged/',ang,'_',var,'.',fileFormat];
fname3 = ['cycleAveraged/',ang,'_',var,'.dat'];
fname4 = ['indiAveraged/',ang,'_',var,'.dat'];
%fname5 = ['averageHovEsc/',ang,'_',var,'.dat'];
fname5 = ['cycleAvg/',ang,'_',var,'.dat'];
fname6 = ['averageHovEsc/',ang,'_',var,'.',fileFormat];
fname7 = ['averageHovEsc/',ang,'_',var,'.',fileFormat2];
titleName = [ang,' ',var1];

%%

cycleCount = 0;
 shades = 1;

for nBird = 1:2
    trialCount = 0;
    if (nBird == 1 )
       vSize = length(bird1_l);
       variable = zeros(vSize,3);
    else
       vSize = length(bird2_l);
       variable = zeros(vSize,3);
    end
for nWing =1:2                     % left = 1 and right = 2
    trialCount = trialCount +1;
    cycleCount = cycleCount +1;

if (nBird == 1 && nWing ==1)
   variable = bird1_l;
elseif (nBird == 1 && nWing ==2)
   variable = bird1_r; 
elseif (nBird == 2 && nWing ==1)
   variable = bird2_l;
elseif (nBird == 2 && nWing ==2)
   variable = bird2_r;
else
    disp('do not continue.')
end

if  (nBird == 1 && nWing ==1)
       %vSize = length(bird1_l);

       downstroke_start_h = [27];
       downstroke_end_h = [45];
       upstroke_start_h = [9];
       upstroke_end_h = [27];

       downstroke_start = [61,88,115,146,177];
       %downstroke_start = [62,88,116,147,178];
       downstroke_end = [74,102,131,163,192];
       %upstroke_start = [45,78,103,134,164];
       upstroke_start = [45,74,102,131,163];
       upstroke_end = [61,88,115,146,177];
elseif (nBird == 1 && nWing ==2)
      % vSize = length(bird1_r);

       downstroke_start_h = [27];
       downstroke_end_h = [45];
       upstroke_start_h = [9];
       upstroke_end_h = [26];

       downstroke_start = [61,89,115,146,178];
       downstroke_end = [75,102,131,163,192];
       %upstroke_start = [45,78,103,134,164];
       upstroke_start = [45,75,102,131,163];
       upstroke_end = [61,89,115,146,178];

    else
       %vSize = length(bird2_l); 

      downstroke_start = [35,61,89,116,143];
       downstroke_end = [49,76,103,131,159];
       %upstroke_start = [20,50,77,104,132];
       upstroke_start = [21,49,76,103,131];
       upstroke_end = [35,61,89,116,143];

end

%---------------------------------Hovering ------------------------------
if (nBird == 1)
numPoints = 51; % Whatever resolution you want.
xCommon_h = linspace(0, 1, numPoints);
wingBeatCount =0;
cycleCommon_h = zeros(length(downstroke_start_h),numPoints);
for nn = 1:length(downstroke_start_h)
wingBeatCount = wingBeatCount +1;
iter = 0;
fullCycle = zeros(downstroke_start_h(nn)-upstroke_end_h(nn)+1,2);
%Upstroke
for n = upstroke_start_h(nn):upstroke_end_h(nn)
    iter = iter +1; 
    fullCycle_h(iter,1) = 1.0*((n-upstroke_start_h(nn))/(2*(upstroke_end_h(nn)-upstroke_start_h(nn))));
    fullCycle_h(iter,2) = variable(n,nAngle);
end
itr_rm = iter;
%downstroke
for n = downstroke_start_h(nn):downstroke_end_h(nn)
    iter = iter +1; 
    fullCycle_h(iter,1) = 0.5+(n-downstroke_start_h(nn))/(2*(downstroke_end_h(nn)-downstroke_start_h(nn)));
    fullCycle_h(iter,2) = variable(n,nAngle);   
end
fullCycle_h(itr_rm +1,:) = []; 
%cycleCommon(cycleCount,:) = interp1(fullCycle(:,1)', fullCycle(:,2)', xCommon);
cycleCommon_h(wingBeatCount,:) = interp1(fullCycle_h(:,1)', fullCycle_h(:,2)', xCommon_h);
end
trial_cat_h(:,cycleCount) =cycleCommon_h(1,:)';

end

%---------------------------------escape ---------------------------------
numPoints = 51; % Whatever resolution you want.
xCommon = linspace(0, 1, numPoints);
wingBeatCount =0;
cycleCommon = zeros(length(downstroke_start),numPoints);
for nn = 1:length(downstroke_start)
wingBeatCount = wingBeatCount +1;
iter = 0;
fullCycle = zeros(downstroke_start(nn)-upstroke_end(nn)+1,2);
%Upstroke
for n = upstroke_start(nn):upstroke_end(nn)
    iter = iter +1; 
    fullCycle(iter,1) = 1.0*((n-upstroke_start(nn))/(2*(upstroke_end(nn)-upstroke_start(nn))));
    fullCycle(iter,2) = variable(n,nAngle);
end
itr_rm = iter;
%downstroke
for n = downstroke_start(nn):downstroke_end(nn)
    iter = iter +1; 
    fullCycle(iter,1) = 0.50+(n-downstroke_start(nn))/(2*(downstroke_end(nn)-downstroke_start(nn)));
    fullCycle(iter,2) = variable(n,nAngle);   
end
fullCycle(itr_rm+1,:) = [];
%cycleCommon(cycleCount,:) = interp1(fullCycle(:,1)', fullCycle(:,2)', xCommon);
cycleCommon(wingBeatCount,:) = interp1(fullCycle(:,1)', fullCycle(:,2)', xCommon);
end
trial_cat(:,cycleCount) =[cycleCommon(1,:)';cycleCommon(2,:)';cycleCommon(3,:)';cycleCommon(4,:)';cycleCommon(5,:)'];
trial_avg(:,cycleCount) = mean(cycleCommon',2);
end
end

cycleAverage = mean(trial_avg,2);
cycleAverage_trial = mean(trial_cat,2);
%noTrial = zeros(length(trial_cat_h),2);
noTrial(1:length(trial_cat_h),1:2) = nan;
trail_hN = [trial_cat_h noTrial];
trial_HE =[trail_hN;trial_cat];
cycleAverage_h = mean(trial_cat_h,2);
cycleAverage_HE = [cycleAverage_h;cycleAverage_trial];
B=linspace(0, 5, 5*numPoints);
B_h = linspace(-1, 5, 6*numPoints);
%B_h = linspace(-1, 0, 1*51);
B_w2 = linspace(-1, 1, 2*numPoints);
trial_avg_w2 = [cycleAverage_h;cycleAverage];
trial_w2 = [trail_hN;trial_avg];
zeroLine= zeros(length(B_h),1);
%%
% time integration to get angular velocitys
% dt = 1e-3;
% cycleAverage_v(1,1)=0.0;
% 
% for i=1:length(cycleAverage)-1   
%     cycleAverage_v(i+1,1)=(cycleAverage_v(i,1)+dt*0.5*(cycleAverage(i+1,1)+cycleAverage(i,1)));        
% end
%%
% figure() 
% set(gca,'FontSize', 18)
% hold on
% %ylim([0 1.5])
% ylim([-150 150])
% %if shades == 1
% grey=[0.25,0.25,0.25]*3.2;
% patch([0.5 1 1 0.5], [max(ylim) max(ylim) min(ylim) min(ylim)],grey)
% 
% plot(xCommon,trial_avg,'k--','LineWidth',0.2)
% plot(xCommon,(cycleAverage),'k','LineWidth',2.0)
% %legend('Downstroke','1st cycle','2nd cycle','cycle average')
% title(titleName)
% xlabel('t/T')
% %xticklabels({''})
% ylabel('angle (deg)')
% % xlim([0 200])
% box on
% grid on
% hold off
% %saveas(gcf,'cycleAveraged/stroke_ang.fig')
% %aveas(gcf,fname1)
% 
% %%
% figure() 
% set(gca,'FontSize', 18)
% hold on
% %ylim([0 1.5])
% ylim([-100 200])
% %if shades == 1
% grey=[0.25,0.25,0.25]*3.2;
% patch([0.5 1 1 0.5], [max(ylim) max(ylim) min(ylim) min(ylim)],grey)
% patch([1.5 2 2 1.5], [max(ylim) max(ylim) min(ylim) min(ylim)],grey)
% patch([2.5 3 3 2.5], [max(ylim) max(ylim) min(ylim) min(ylim)],grey)
% patch([3.5 4 4 3.5], [max(ylim) max(ylim) min(ylim) min(ylim)],grey)
% patch([4.5 5 5 4.5], [max(ylim) max(ylim) min(ylim) min(ylim)],grey)
% 
% plot(B,trial_cat,'k--','LineWidth',0.2)
% plot(B,(cycleAverage_trial),'k','LineWidth',2.0)
% %legend('Downstroke','1st cycle','2nd cycle','cycle average')
% title(titleName)
% xlabel('t/T')
% %xticklabels({''})
%  ylabel('Angle (deg)')
% % xlim([0 200])
% box on
% grid on
% %saveas(gcf,'indiAveraged/stroke_ang.fig')
% %saveas(gcf,fname2)
% 
% %%
% % ---------------------------Including hovering cycle --------------------
% 
% figure() 
% set(gca,'FontSize', 18)
% hold on
% %ylim([0 1.5])
% ylim([-150 150])
% %if shades == 1
% grey=[0.25,0.25,0.25]*3.2;
% patch([-0.5 0 0 -0.5], [max(ylim) max(ylim) min(ylim) min(ylim)],grey)
% patch([0.5 1 1 0.5], [max(ylim) max(ylim) min(ylim) min(ylim)],grey)
% 
% plot(B_w2,trial_w2 ,'k--','LineWidth',0.2)
% plot(B_w2,trial_avg_w2,'k','LineWidth',2.0)
% %legend('Downstroke','1st cycle','2nd cycle','cycle average')
% title(titleName)
% xlabel('t/T')
% %xticklabels({''})
% ylabel('angle (deg)')
% % xlim([0 200])
% box on
% grid on
% hold off
% %saveas(gcf,'cycleAveraged/stroke_ang.fig')
% %aveas(gcf,fname1)

%%
figure() 
%set(gca,'FontSize', 20)
axes('FontName','Times','FontSize',20)
hold on
%ylim([0 1.5])
ylim([-0.4 0.4])
%if shades == 1
grey=[0.25,0.25,0.25]*3.2;
patch([-0.5 0 0 -0.5], [max(ylim) max(ylim) min(ylim) min(ylim)],grey,'LineStyle','none')
patch([0.5 1 1 0.5], [max(ylim) max(ylim) min(ylim) min(ylim)],grey,'LineStyle','none')
patch([1.5 2 2 1.5], [max(ylim) max(ylim) min(ylim) min(ylim)],grey,'LineStyle','none')
patch([2.5 3 3 2.5], [max(ylim) max(ylim) min(ylim) min(ylim)],grey,'LineStyle','none')
patch([3.5 4 4 3.5], [max(ylim) max(ylim) min(ylim) min(ylim)],grey,'LineStyle','none')
patch([4.5 5 5 4.5], [max(ylim) max(ylim) min(ylim) min(ylim)],grey,'LineStyle','none')

plot(B_h,trial_HE,'k--','LineWidth',0.2)
plot(B_h,(cycleAverage_HE),'k','LineWidth',2.0)
% plot(B_h,trial_cat_h,'k--','LineWidth',0.2)
% plot(B_h,(cycleAverage_h),'k','LineWidth',2.0)
% plot(B,trial_cat,'k--','LineWidth',0.2)
% plot(B,(cycleAverage_trial),'k','LineWidth',2.0)
%legend('Downstroke','1st cycle','2nd cycle','cycle average')
plot(B_h,zeroLine(:,1),'k--','LineWidth',1.5)
title(titleName)

xlabel('t/T','FontName','Times','FontSize',20)
xlim([-1 5])
%xticklabels({''})
 ylabel('C_p','FontName','Times','FontSize',20')
% xlim([0 200])
box on
%grid on

%saveas(gcf,fname6)


%%
% save indiAveraged/stroke_ang.dat trial_cat -ascii   
% 
% save cycleAveraged/stroke_ang.dat trial_avg -ascii  

% save(fname3,'trial_avg','-ascii')
% save(fname4,'trial_cat','-ascii')

%save(fname5,'trial_HE','-ascii')

%save(fname5,'cycleAverage_HE','-ascii')

%%
%n1 = [1 53 104 155 205 255];
%n2 = [52 103 154 204 255 306 ];

n1 = [1 52 103 154 205 255];
n2 = [51 102 153 204 255 306 ];

for n = 1:length(n1)

    IndAvg(n,:) = mean(trial_HE(n1(n):n2(n),:));
    avgAvg(n,:) = mean(cycleAverage_HE(n1(n):n2(n),:));
end

%%

%        downstroke_start_h = [27];
%        downstroke_end_h = [45];
%        upstroke_start_h = [9];
%        upstroke_end_h = [26];
% 
%        downstroke_start = [62,88,116,147,178];
%        downstroke_end = [74,102,131,163,192];
%        %upstroke_start = [45,78,103,134,164];
%        upstroke_start = [45,74,102,131,163];
%        upstroke_end = [61,88,115,146,177];


n_off = 1;
n1_b1 = [46 75 102 133 165] + n_off;
n2_b1 = [74 101 132 164 191] + n_off;

n1_b2 = [20 50 76 103 131]+n_off;
n2_b2 = [49 75 102 130 158]+n_off;

IndAvg_hovP(1,1) = mean(bird1_l(8:45,3));
IndAvg_hovP(1,2) = mean(bird1_r(8:45,3));

IndAvg_hovS(1,1) = mean(bird1_l(8:45,1));
IndAvg_hovS(1,2) = mean(bird1_r(8:45,1));

for n = 1:length(n1_b1)

    IndAvg_escP(n,1) = mean(bird1_l(n1_b1(n):n2_b1(n),3));
    IndAvg_escP(n,2) = mean(bird1_r(n1_b1(n):n2_b1(n),3));
    IndAvg_escP(n,3) = mean(bird2_l(n1_b2(n):n2_b2(n),3));
    IndAvg_escP(n,4) = mean(bird2_r(n1_b2(n):n2_b2(n),3));

    IndAvg_escS(n,1) = mean(bird1_l(n1_b1(n):n2_b1(n),1));
    IndAvg_escS(n,2) = mean(bird1_r(n1_b1(n):n2_b1(n),1));
    IndAvg_escS(n,3) = mean(bird2_l(n1_b2(n):n2_b2(n),1));
    IndAvg_escS(n,4) = mean(bird2_r(n1_b2(n):n2_b2(n),1));
    
end
%%
ratio_per = 100*IndAvg_escP./IndAvg_escS;

bird1 = ones(5,1);
bird2 = 2*ones(5,1);
xRange = 0:3;
zeroLine_x= zeros(length(xRange),1);
figure() 
%set(gca,'FontSize', 20)
axes('FontName','Times','FontSize',20)
hold on

ylim([-20 21])


plot(bird1,ratio_per(:,1),'b o','LineWidth',2.0)
plot(bird1,ratio_per(:,2),'r o','LineWidth',2.0)
plot(bird2,ratio_per(:,3),'b o','LineWidth',2.0)
plot(bird2,ratio_per(:,4),'r o','LineWidth',2.0)
plot(xRange,zeroLine_x,'k--','LineWidth',1.5)
title('pitch to stroke power')

%xlabel('t/T','FontName','Times','FontSize',20)
xlim([0 3])
%xticklabels({''})
xticklabels({'','Bird1','Bird2',''})
 ylabel('% ','FontName','Times','FontSize',20')
% xlim([0 200])
box on
%grid on

%saveas(gcf,fname6)



